#include "common.h"
#include <signal.h>
#include <sched.h>
#include <unistd.h>
#include <sys/stat.h>
// c++ head file
#include <iostream>
#include <string>
#include <cstdlib>
#include <unordered_set>
#include <vector>
#include <mutex>
#include <condition_variable>
#include <iomanip>
#include <atomic>
#include <memory>
#include <gnss.h>
#include <sonar.h>

// 全局变量声明
std::mutex gnss_data_mutex;
std::condition_variable gnss_data_cv;
struct gnss_data gnss_latest_data;

std::mutex sonar_data_mutex;
std::condition_variable sonar_data_cv;
struct sonar_data sonar_latest_data;

gnss_data& gn = gnss_latest_data; 
sonar_data& sn = sonar_latest_data; 

// 添加全局原子标志位
std::atomic<bool> g_shutdown_requested{false};

void* ThreadFuncWrapper(void* arg);
void* ThreadFunc(SensorAbstraction* sensor_opt);

bool SensorAbstraction::RunOK = true;

SensorAbstraction::SensorAbstraction(const std::string& sensor_name_,const int& run_mode_):
                                    sensor_name(sensor_name_),run_mode(run_mode_){}

SensorAbstraction::~SensorAbstraction(){
        
        std::cout<<sensor_name<<": ~SensorAbstraction()"<<std::endl;
}
int SensorAbstraction::Run(){
    //如果是数据采集模式
    std::cout<<"run_mode:"<<run_mode<<"runOk:"<<RunOK<<std::endl;
    if(run_mode == 2){
        while(RunOK){
	        if(DataCollection()||DataProcessing()||DataStorage()){
		        return -1;
	        }
	    }
    }else if(run_mode == 1){
        while(RunOK){
	        if(DataCollection()||DataProcessing()||DataPrint()){
		        return -1;
	        }
	    }
    }

	return 0;
}
void SensorAbstraction::ThreadInit(int sche_algo,int pth_priority){
    //线程初始化
    pthread_attr_init(&attr_);
    //设置该线程由主线程回收其资源
    pthread_attr_setdetachstate(&attr_,PTHREAD_CREATE_JOINABLE);  //退出后保留完成状态
    //设置线程不继承调用该线程的线程属性
    pthread_attr_setinheritsched(&attr_,PTHREAD_EXPLICIT_SCHED);  //使用自定义调度策略和参数
    //线程优先级设置
    param_.sched_priority=pth_priority;                           //优先级 全是1
    //设置线程调度策略
    pthread_attr_setschedpolicy(&attr_,sche_algo);                //线程调度策略：全是循环策略
    pthread_attr_setschedparam(&attr_,&param_);
}
void SensorAbstraction::StartThread(){
    int ret = pthread_create(&tid, &attr_, ThreadFuncWrapper, this);
    if(ret < 0){
        std::cerr<<sensor_name<<": thread create fail"<<std::endl;
        return;
    }
    pthread_detach(tid);
    std::cout<<sensor_name<<": thread create successful"<<std::endl;
    
}

void* ThreadFuncWrapper(void* arg){
    // std::cout<<"ThreadFuncWrapper"<<std::endl;
    SensorAbstraction* sensor_opt=static_cast<SensorAbstraction*>(arg);   //static_cast 强制类型转化 将传入的子类指针强制转换成父类指针
    ThreadFunc(sensor_opt);

}
void* ThreadFunc(SensorAbstraction* sensor_opt){
    std::cout<<"sensot init"<<std::endl;
    //初始化传感器
    if(sensor_opt->Init()){
        //结束线程并退出
        std::cout<<sensor_opt->sensor_name<<": thread init fail"<<std::endl;
        
    }
    if(sensor_opt->Run()){
        std::cout<<sensor_opt->sensor_name<<": thread run fail"<<std::endl;
        
    };
    std::cout<<sensor_opt->sensor_name<<": thread exit"<<std::endl;
}

void signalHandler(int sign_num){
    SensorAbstraction::RunOK =false;
    g_shutdown_requested.store(true);
    std::cout<<"\nreceive signal [SIGTSTP]!"<<std::endl;
    // exit(0);
}

void printUsage(){
    std::cout<<"Usage: "<<std::endl;
    std::cout<<"print sensor data: ./sonar -p sensor_name[sonar gnss]"<<std::endl;
    std::cout<<"save sensor data: ./sonar -g file_num[1 2 3] sensor_name[sonar gnss]"<<std::endl;   
}
std::shared_ptr<start_para> InitSystem(int argc,char** argv){
    signal(SIGINT, signalHandler);
   
    std::unordered_set<std::string> name_set = {"sonar", "gnss"};
    if(argc < 3){
        printUsage();
        return nullptr;
    }
    std::shared_ptr<start_para>p_start_para = std::make_shared<start_para>(); ;
    int start_index = 0;
    std::string mode=argv[1];
    if(mode == "-p"){
        p_start_para->run_mode_ = 1;
        start_index = 2;
    }else if(mode == "-g"){
        p_start_para->run_mode_ = 2;
        if(!atoi(argv[2])){
            // not digit
            std::cout<<"main: please input correct file num"<<std::endl;
            printUsage();
            return nullptr;
        }
        p_start_para->file_num_ = atoi(argv[2]);
        // 检查文件是否存在
        std::string file_name = "data_" + std::to_string(p_start_para->file_num_) + ".csv";
        struct stat buffer;
        if(stat(file_name.c_str(), &buffer) == 0){
            std::cout<<"main: file already exist, please input correct file num"<<std::endl;
            printUsage();
            return nullptr;
        }
        p_start_para->p_ofs_ = std::make_unique<std::ofstream>(file_name, std::ios::out);
        if(!p_start_para->p_ofs_->is_open()) {
            std::cout<<"main: open file_name fail"<<std::endl;
            return nullptr;
        }
        // 控制浮点数精度
        *p_start_para->p_ofs_ << std::fixed << std::setprecision(6);
        start_index = 3;
    }else{
        printUsage();
        return nullptr;
    }
    for(int i = start_index; i < argc; i++){
        std::string sensor_name = argv[i];
        if(name_set.find(sensor_name) != name_set.end()){
            p_start_para->sensor_list_[sensor_name] = 1;
        }else{
            printUsage();
            return nullptr;
        }
    }
    
    // print info
    std::cout<<"main: run_mode: "<<p_start_para->run_mode_<<std::endl;
    std::cout<<"main: file_num: "<<p_start_para->file_num_<<std::endl;
    for(const auto& sensor :p_start_para->sensor_list_){
        std::cout<<"main: sensor: "<<sensor.first<<std::endl;
    }
    
    return p_start_para;

}
//save sensor share_ptr
std::vector<std::shared_ptr<SensorAbstraction>> sensor_vec;
std::shared_ptr<Gnss>p_gnss;
std::shared_ptr<Sonar>p_sonar;
void StartSystem(std::shared_ptr<start_para> p_start_para){
    if(p_start_para == nullptr) return;
    for(const auto& sensor :p_start_para->sensor_list_){
        if(sensor.first == "sonar"){
            #ifdef SONAR_TYPE1
            p_sonar = std::make_shared<Sonar>(sensor.first, p_start_para->run_mode_, "/dev/ttyACM0");
            #else  
            p_sonar = std::make_shared<Sonar>(sensor.first, p_start_para->run_mode_, "/dev/ultrasound_serial2");
            #endif
            sensor_vec.emplace_back(p_sonar);
            p_sonar->ThreadInit(SCHED_RR, 1);
            p_sonar->StartThread();
        }
        else if(sensor.first == "gnss"){
            p_gnss = std::make_shared<Gnss>(sensor.first, p_start_para->run_mode_, "/dev/ultrasound_serial1");
            sensor_vec.emplace_back(p_gnss);
            p_gnss->ThreadInit(SCHED_RR, 1);
            p_gnss->StartThread();
        }
    }
    bool first_run = true;
    size_t flush_count = 0;
    long int last_cnt = -1;
    while(!g_shutdown_requested.load()){
        if(p_start_para->run_mode_ == 2){
            // 获取gnss的最新数据，和声呐的最新数据，然后保存到文件
            if(sensor_vec.size() == 2){
                // 2个传感器都有
                if(first_run){
                    *p_start_para->p_ofs_<<"gnrmc_status,gngga_status,satellites_num,datetime,latitude,longitude,altitude,waterDepth"<<std::endl;
                    first_run = false;
                }
                {
                    std::unique_lock<std::mutex> gnss_lock(gnss_data_mutex);
                    gnss_data_cv.wait(gnss_lock); // 等待通知
                    if(gn.cnt != last_cnt && gn.gnrmc_status == 1 && gn.gngga_status == 1 ){
                        *p_start_para->p_ofs_<<gn.gnrmc_status<<","<<gn.gngga_status<<","<<gn.satellites_num<<","<<gn.datetime<<","<<gn.latitude<<","<<gn.longitude<<","<<gn.altitude<<","<<sn.water_depth<<std::endl;
                        last_cnt = gn.cnt;
                        std::cout<<"main: saving gnss data, cnt: "<<gn.cnt<<" longitude: "<<gn.longitude<<" latitude: "<<gn.latitude<<" altitude: "<<gn.altitude<<" water depth: "<<sn.water_depth<<std::endl;
                    }
                   
                }
            }else if(sensor_vec[0] == p_gnss){
                // 只有gnss
                if(first_run){
                    *p_start_para->p_ofs_<<"gnrmc_status,gngga_status,satellites_num,datetime,latitude,longitude,altitude"<<std::endl;
                    first_run = false;
                }
                {
                    std::unique_lock<std::mutex> gnss_lock(gnss_data_mutex);
                    gnss_data_cv.wait(gnss_lock); // 等待通知
                    if(gn.cnt != last_cnt && gn.gnrmc_status == 1 && gn.gngga_status == 1 ){
                        *p_start_para->p_ofs_<<gn.gnrmc_status<<","<<gn.gngga_status<<","<<gn.satellites_num<<","<<gn.datetime<<","<<gn.latitude<<","<<gn.longitude<<","<<gn.altitude<<std::endl;
                        last_cnt = gn.cnt;
                        std::cout<<"main: saving gnss data, cnt: "<<gn.cnt<<" longitude: "<<gn.longitude<<" latitude: "<<gn.latitude<<" altitude: "<<gn.altitude<<std::endl;
                    }
                   
                }
                
              

            }else{
                // 只有声呐
                if(first_run){
                    *p_start_para->p_ofs_<<"cnt,waterDepth"<<std::endl;
                    first_run = false;
                }
                {
                    std::unique_lock<std::mutex> sonar_lock(sonar_data_mutex);
                    sonar_data_cv.wait(sonar_lock); // 等待通知
                    if(sn.cnt != last_cnt ){
                        *p_start_para->p_ofs_<<sn.cnt<<","<<sn.water_depth<<std::endl;
                        last_cnt = sn.cnt;
                        std::cout<<"main: saving sonar data, cnt: "<<sn.cnt<<" water depth: "<<sn.water_depth<<std::endl;
                    }
                   
                }
                
            }
            // 定期刷新
            if(++flush_count %20 == 0){
                p_start_para->p_ofs_->flush();
            }
        }else{
            sleep(1);
            //std::cout<<"main: print mode"<<std::endl;
        }
    }
    // 程序退出处理
    std::cout<<"main: shutdown requested"<<std::endl;
    if(p_start_para->run_mode_ == 2 &&p_start_para->p_ofs_){
        p_start_para->p_ofs_->flush();
        p_start_para->p_ofs_->close();
    }

}